import numpy as np, scipy.special as sp, scipy.integrate as ig, copy, re, math, os
from utils.polygon.ConvexHull import ConvexHull
from utils.polygon.Polygon import Polygon
from Model import Model,Actor,Evidence,TrackSegment,Node,TimeNode,Time,Distributions
from segmentation.Formatter import Formatter

class Frame(object):

    def __init__(self,row):
        self.actorid = row[0]
        self.xmin = row[1]
        self.ymin = row[2]
        self.xmax = row[3]
        self.ymax = row[4]
        self.frame = row[5]
        self.valid = row[6]  # if 0, then valid
        self.occluded = row[7]
        self.generated = row[8]
        self.label = row[9]
        
        if len(row) > 10:
            self.regions = row[10:len(row)]
    
    def toRow(self):
        return [self.actorid,self.xmin,self.ymin,self.xmax,self.ymax, \
                self.frame,self.valid,self.occluded,self.generated,self.label, \
                self.regions]
        
    def __str__(self):
        return str(self.toRow())
    
    def length(self):
        return len(self.toRow())


def read_data(infile):
    with open(infile, 'r') as f:
        content  = f.readlines()
        data = dict()
        count = 0
        for row in content:
            if count > 25000:
                break
            row = row.strip().split(" ")
            actorid = row[0]
            fr = Frame(row)
            
            if int(fr.valid) == 1:
                continue

            if data.has_key(actorid):
                data[actorid].append(fr)
            else:
                data[actorid] = [fr]
            
            count += 1
  
    return data


class CrossingCrosswalk:
    
    def __init__(self):
        # Coordinate for crosswalk (scaled by 1/2)
        self.CrossWalkPoints = [(70,221), (180,445), (0,294), (109,540)]
    
        self.model = Model()
        self.model.name = "Xingxwalk"
        self.model.subscriptions = [""]
        self.model.topNode = "xingxwalkmain"
        self.model.independent = True

        act1 = Actor()
        act1.name = "dummy1"
        act1.atype = Actor.NULL
        
        tempNode = Node()
        tempNode.getID()
        tempNode.name = "xingxwalkmain"
        tempNode.subjects = [act1]
        tempNode.objects = []
        tempNode.evvecs = []
        tempNode.bound = False

        self.model.topNode = tempNode.name
        self.model.nodes.append(copy.deepcopy(tempNode))

        tempNode.name = "xingxwalksub"
        tempEv = TrackSegment()
        tempEv.actor = act1
        tempEv.segtype = Evidence.MOVE
        tempEv.willBind = True
        tempEv.endsNode = True
        tempEv.bound = False
        
        tempNode.evvecs = [copy.deepcopy(tempEv)]
        
        def calculate_xwalk_region(regions):
            count = 0.0
            for fr in regions:
                if "crosswalk" in set(fr):
                    count += 1
            return count / len(regions)
        
        def angle2D(v1, v2):
            """
            Returns the angle (in degrees) between two 2D vectors
            """
            return math.degrees( math.atan2(v1[0]*v2[1]-v2[0]*v1[1], v1[0]*v2[0]+v1[1]*v2[1]) )
        
        def magnitude(v):
            return math.sqrt(sum(v[i]*v[i] for i in range(len(v))))
        
        def normalize(v):
            vmag = magnitude(v)
            return v if vmag == 0.0 else np.array([v[i] / vmag  for i in range(len(v))])

        def calculate_xwalk_heading(track1):
            """
            Returns the percentage of track's direction and cross-walk are in isparallel
            """
            locations = track1.locations  
            xpts = []; zpts = []
            for l in locations:
                xpts.append(l[0])
                zpts.append(l[1])
            
            window = 10                      # using 5 frames to calculate average
            while (len(xpts) < 2*window):
                window = window / 2
            if window <= 1:
                return [0]
            
            x_ave = []; z_ave = []
            for i in range(0, len(xpts)-window+1, window/2):
                x_win = xpts[i:i+window]
                z_win = zpts[i:i+window]
                x_ave.append( sum(x_win)/window )
                z_ave.append( sum(z_win)/window )
            
            """ cross-walk boundaries """
            edge1 = 2*np.array(self.CrossWalkPoints[0:2])
            edge2 = 2*np.array(self.CrossWalkPoints[2:4])
            v1 = edge1[1] - edge1[0]
            v2 = edge2[1] - edge2[0]
            xwalkvec_ave = (normalize(v1) + normalize(v2))/2.0
            
            
            isparallel = []
            for i in range(len(x_ave)-1):
                xz = np.array([ (x_ave[i],z_ave[i]),(x_ave[i+1],z_ave[i+1]) ])
                xz_vec = xz[1] - xz[0]
                
                angle_ave = angle2D(xwalkvec_ave, xz_vec)
                
                if (angle_ave >= 0) and (angle_ave <= 90):
                    isparallel.append(angle_ave)
                elif (angle_ave > 90) and (angle_ave <= 180):
                    diff = 180 - angle_ave
                    isparallel.append(diff)
                elif (angle_ave >= -180) and (angle_ave <= -90):
                    diff = 180 + angle_ave 
                    isparallel.append(diff)
                elif (angle_ave > -90) and (angle_ave <= 0):
                    diff = -angle_ave
                    isparallel.append(diff)
                else:
                    raise Exception(str(angle_ave))
            
            return isparallel
                  
            
        def xwalkJoint(evvecs):
            track = evvecs[0]
            
            # calculate region
            avgpctxwalk = calculate_xwalk_region(track.regions)
            
            # calculate track heading
            parallel = calculate_xwalk_heading(track)
            
            SMALLEST_DECIMAL = np.nextafter(0,1)
            
            meanparallel = sum(parallel)/len(parallel)
            mweight = CrossingCrosswalk.meanDensity(meanparallel)
            
            stdparallel = np.std(parallel)
            sweight = CrossingCrosswalk.stdDensity(stdparallel)
            
            time = (track.endtime - track.starttime)/Model.FR/60.
            timeWeight = CrossingCrosswalk.timeDensity(max(time, 1))
            
            totalweight = np.log(max(avgpctxwalk,SMALLEST_DECIMAL)) + mweight + sweight + timeWeight
            
            return totalweight,.5
        
        tempNode.lfunction = xwalkJoint

        self.model.nodes.append(copy.deepcopy(tempNode))

    @staticmethod
    def meanDensity(mean):
        weight = Distributions.PDFs.foldednorm(mean,0.7,15) 
        return weight

    @staticmethod
    def stdDensity(std):
        weight = Distributions.PDFs.foldednorm(std,0.5,8) 
        return weight
    
    @staticmethod
    def timeDensity(time):
        weight = Distributions.PDFs.gamma(time,0.20,2.5)
        return weight



if __name__ == "__main__":
    
    path = '/Users/tpilanta/Documents/workspace/python/ua-bb-model-framework-2013'
    polfile = os.path.join(path,'pol-crosswalk.txt')
    datadict = read_data(polfile)
    
    myFormatter = Formatter()
#    actorList = myFormatter.process_pol_tracks_from_file(os.path.join(path,"./pol.txt"))   
    actorList = myFormatter.process_pol_tracks_from_file(os.path.join(path,"./pol-crosswalk.txt"))

    myWalk = CrossingCrosswalk()
    myWalk.model.name
    myWalk.model.nodes
    for act in actorList:
        actorid = act.name.split('_')[1]
                
        if datadict.has_key(actorid):
            
            # Make track segment evidence    
            trackseg = TrackSegment(actor = act,starttime = act.starttime,endtime = act.endtime,segtype = Evidence.MOVE_SLOW)
            trackseg.locations = act.locations[:]
            
            # Make regions evidence
            regions = []
            for row in datadict[actorid]:
                assert row.length() == 11, str(row)
                r = row.toRow()[10]
                r_new = []
                for region in r:
                    region = region.replace("\"","")
                    r_new.append(region)
                regions.append(r_new)
            
            evvecs = [trackseg, regions]
            
            w = myWalk.model.nodes[1].lfunction(evvecs)
            print w, actorid
            

        
        
#        myWalk.model.evaluate_evidence()
#        myWalk.model.print_xml(outf = "outloiter/loiter", mfiles = True)
        
        
#    points = []
#    
#    for actorid in datadict.keys():
#        for row in datadict[actorid]:
#            assert row.length() == 11, str(row)
#            regions = row.toRow()[10]
#            for region in regions:
#                region = region.replace("\"","")
#                if not region == "crosswalk":
#                    continue
#                points.append((row.xmin,row.ymin,row.xmax,row.ymax))

